import numpy
import matplotlib.pyplot as plt
import pandas
import json
import os
import utm
from math import pi, floor

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# 角度限制, 输入弧度, 输出-pi~pi
def rad_limit(rad: float) -> float:
	if rad >= 180:
		rad = rad - 360
	if rad < -180:
		rad = rad + 360
	return rad

# json to dict?
def json_decode(df_data: pandas.DataFrame):
    columns_name = []
    for key in json.loads(df_data.iloc[0]):
        columns_name.append(key)
    
    temp = pandas.DataFrame(columns=columns_name)
    for i in range(len(df_data)):
        temp_dict = json.loads(df_data.iloc[i])
        temp_series = pandas.Series(temp_dict).to_frame()
        temp = pandas.concat([temp, temp_series.T])
    return temp

def cal_psi_error(data_df: pandas.DataFrame):
    data_df['psi_error'] = data_df['psi'] - data_df['exp_psi']
    for i in range(len(data_df['psi_error'])):
        data_df['psi_error'].iloc[i] = rad_limit(data_df['psi_error'].iloc[i])
    return data_df

if __name__ == '__main__':
    HOME = os.getcwd()
    
    exp_line_df = pandas.read_csv(f'{HOME}/usv_simulator/navigation/gps_L_path.csv')
    # print(exp_line_df)

    pid_data = pandas.DataFrame()

    exp_line_east = []
    exp_line_north = []
    init_position = [.0, .0]
    for i in range(len(exp_line_df['lon'])):
        lon, lat = exp_line_df['lon'].to_list()[i], exp_line_df['lat'].to_list()[i]
        [east, north, zone_number, zone_letter] = utm.from_latlon(latitude=lat, longitude=lon)
        if i == 0:
             init_position = [east, north]
        exp_line_east.append(east - init_position[0])
        exp_line_north.append(north - init_position[1])
    exp_line_df['east'] = exp_line_east
    exp_line_df['north'] = exp_line_north

    # print(exp_line_df['east'])
    # print(exp_line_df['north'])

    data_df = pandas.read_csv(f'{HOME}/usv_simulator/navigation/l_PATH.csv')
    # print(data_df)
    lon_path = []
    lat_path = []
    data_df = data_df["data"][850:1600].to_list()
    for point in data_df:
        lon_path.append(json.loads(point)['ShipInfo'][0]['lon'])
        lat_path.append(json.loads(point)['ShipInfo'][0]['lat'])

    pid_data_east = []
    pid_data_north = []
    for i in range(len(lon_path)):
        lon, lat = lon_path[i], lat_path[i]
        [east, north, zone_number, zone_letter] = utm.from_latlon(latitude=lat, longitude=lon)
        pid_data_east.append(east - init_position[0] - 15)
        pid_data_north.append(north - init_position[1] + 10)

    fig,ax = plt.subplots()
    # plt.rcParams['font.size']=15
    ax.plot(exp_line_df['east'].to_numpy(), exp_line_df['north'].to_numpy(), label='Reference Path')
    ax.plot(pid_data_east, pid_data_north, label='Tracking Path')
    ax.legend()
    ax.set_xlabel('x / m')
    ax.set_ylabel('y / m')
    # ax[0,0].set_ylim(29.64, 29.66)
    
    # plt.savefig(f'{HOME}/data_deal/plot_path.eps', bbox_inches='tight', dpi=300)
    plt.show()